#include "vio/tools/math_utils.h"

namespace vio {
namespace math_utils {

double SampsonusError(const Eigen::Vector2d& v2Dash,
                      const Eigen::Matrix3d& Essential,
                      const Eigen::Vector2d& v2) {
  Eigen::Vector3d v3Dash = unproject2d(v2Dash);
  Eigen::Vector3d v3 = unproject2d(v2);

  double dError = v3Dash.transpose() * Essential * v3;

  Eigen::Vector3d fv3 = Essential * v3;
  Eigen::Vector3d fTv3Dash = Essential.transpose() * v3Dash;

  Eigen::Vector2d fv3Slice = fv3.head<2>();
  Eigen::Vector2d fTv3DashSlice = fTv3Dash.head<2>();

  return (dError * dError /
          (fv3Slice.dot(fv3Slice) + fTv3DashSlice.dot(fTv3DashSlice)));
}

// A = [x1, -R*x2]
// A * [s1, s2]' = t
// s1 * f1 = s2 * R * f2 + t
Eigen::Vector3d TriangulateFeatureNonLin(const Eigen::Matrix3d& R,
                                         const Eigen::Vector3d& t,
                                         const Eigen::Vector3d& feature1,
                                         const Eigen::Vector3d& feature2) {
  Eigen::Vector3d f2 = R * feature2;
  Eigen::Vector2d b;
  b[0] = t.dot(feature1);
  b[1] = t.dot(f2);
  Eigen::Matrix2d A;
  A(0, 0) = feature1.dot(feature1);
  A(1, 0) = feature1.dot(f2);
  A(0, 1) = -A(1, 0);
  A(1, 1) = -f2.dot(f2);
  Eigen::Vector2d lambda = A.inverse() * b;
  Eigen::Vector3d xm = lambda[0] * feature1;
  Eigen::Vector3d xn = t + lambda[1] * f2;
  return (xm + xn) / 2;
}

double ReprojError(const Eigen::Vector3d& f1,
                   const Eigen::Vector3d& f2,
                   double error_multplier2) {
  Eigen::Vector2d error = project2d(f1) - project2d(f2);
  return error_multplier2 * error.norm();
}

// triangulate features, calculate reproject error (outliers and inliers)
double ComputeInliers(const std::vector<Eigen::Vector3d>& features1,
                      const std::vector<Eigen::Vector3d>& features2,
                      const Eigen::Matrix3d& R,
                      const Eigen::Vector3d& t,
                      const double reproj_thresh,
                      double error_multiplier2,
                      std::vector<Eigen::Vector3d>& xyz_vec,
                      std::vector<int>& inliers,
                      std::vector<int>& outliers) {
  inliers.clear();
  inliers.reserve(features1.size());
  outliers.clear();
  outliers.reserve(features1.size());
  xyz_vec.clear();
  xyz_vec.reserve(features1.size());
  double tot_error = 0;
  // triangulate all features and compute reprojection errors and inliers
  for (size_t j = 0; j < features1.size(); j++) {
    xyz_vec.push_back(
        TriangulateFeatureNonLin(R, t, features1[j], features2[j]));
    double e1 = ReprojError(features1[j], xyz_vec.back(), error_multiplier2);
    double e2 = ReprojError(
        features2[j], R.transpose() * (xyz_vec.back() - t), error_multiplier2);
    if (e1 > reproj_thresh || e2 > reproj_thresh) {
      outliers.push_back(j);
    } else {
      inliers.push_back(j);
      tot_error += e1 + e2;
    }
  }
  return tot_error;
}

}  // namespace math_utils
}  // namespace vio
